
#include "skeleton.h"
#include <iterator>

/// This struct is used to compute the dijkstra algorithm
struct Joint_id_d {
   
   Joint_id_d(): id(0), d(0) {}
   Joint_id_d(unsigned int i, Number di): id(i), d(di) {}
   
   int     id;    // id of skeleton's joint
   Number   d;    // distance value on vertex id-th
};

// functor to compare skeleton's joints 
struct compareJointDistance {
   bool operator() (const Joint_id_d &a, const Joint_id_d &b) const {
      return a.d > b.d;
   }
};

Skeleton::Skeleton() {
   
   joints.reserve(100);
   bones.reserve(100);
   graph.reserve(100);
   k.reserve(100);
   min_distance_vertex.reserve(100);
   closest_vertex.reserve(100);
   manOrAutowab.reserve(100);
   //~ wab.reserve(100);
}

Skeleton::Skeleton(const Skeleton& s) {
   
   joints = s.joints;
   bones = s.bones;
   graph = s.graph;
   
   d = s.d;
   k = s.k;
   //wab = s->wab;
   manOrAutowab = s.manOrAutowab;
   closest_vertex = s.closest_vertex; 
   min_distance_vertex = s.min_distance_vertex; 
   dbones = s.dbones; 
}

Skeleton::~Skeleton() {
   
}

Skeleton::Skeleton(const char* filename) {
   
   string skelformat; unsigned int njoints;
   std::ifstream skelfile(filename);
   if (not skelfile.is_open()) {
      std::cout<<"I/O error: grrr the skeleton file doesn't exist "<<std::endl;
      return;
   }
   
   // Load header
   skelfile>>skelformat;
   assert(skelformat=="SKEL");
   skelfile>>njoints;
   
   // Reserve memory
   joints.reserve(njoints*2);
   bones.reserve(njoints*2);
   graph.reserve(njoints*2);
   
   // Load the data
   Number x, y, z; int id, idprev;
   for (unsigned int i=0; i<njoints; ++i) {
      skelfile>>id>>x>>y>>z>>idprev;
      joints.push_back(Point3(x,y,z));
      bones.push_back(Bone(id,idprev));
   }
   // Built the connectivity graph
   buildGraph();
   
   // Initialize skinning variables 
   d.resize(njoints);
   k.resize(njoints, false);
   min_distance_vertex.resize(njoints, 10e8);
   closest_vertex.resize(njoints, -1);
   manOrAutowab.resize(njoints, false);
   //~ wab.resize(njoints);
   //~ for (int i=0; i<wab.size(); ++i) {
      //~ wab[i].resize(1,1);
   //~ }
   
   computeBonesDistanceTable();
   
   vector<vector<int> > &c = controller;
   c.resize(bones.size());
   c[1].resize(2);  c[1][0] = 12; c[1][1] = 15;
   c[2].resize(2);  c[2][0] = 4;  c[2][1] = 8;
   c[3].resize(2);  c[3][0] = 12; c[3][1] = 15;
   c[4].resize(1);  c[4][0] = 2; 
   c[5].resize(2);  c[5][0] = 7; c[5][1] = 4;        //wab[5].resize(2); wab[5][0] = 0.3; wab[5][1] = 0.7;
   c[6].resize(1);  c[6][0] = 7;
   c[7].resize(1);  c[7][0] = 6;
   c[8].resize(1);  c[8][0] = 2; 
   c[9].resize(2);  c[9][0] = 11; c[9][1] = 8;        //wab[9].resize(2); wab[9][0] = 0.3; wab[9][1] = 0.7;
   c[10].resize(1); c[10][0] = 11;
   c[11].resize(1); c[11][0] = 10;
   c[12].resize(1); c[12][0] = 1;
   c[13].resize(1); c[13][0] = 3;
   c[14].resize(1); c[14][0] = 3;
   c[15].resize(1); c[15][0] = 1;
   c[16].resize(1); c[16][0] = 3;
   c[17].resize(1); c[17][0] = 3;
   
   atbones.clear();
   atbones.resize(bones.size(), Vector3(0,0,0));
   
   const float LIMINF = 1e-1;
   for (int i=1; i<bones.size(); ++i) {
      //~ cout<<"i: "<<i<<endl;
      const Bone &bi = bones[i];
      Vector3 unibi = joints[bi.first]-joints[bi.second];
      unibi = unibi/sqrt(unibi*unibi);
      
      for (int j=0; j<c[i].size(); ++j) {
         Number d = getDistanceBetweenBone(i,c[i][j]);
         int k = 1;
         if (d>0) {
            while ((getDistanceBetweenBone(i,k)>0) or 
                   (getDistanceBetweenBone(c[i][j],k)>=d)) {
               k++;
            }
            
            const Bone &bk = bones[k];
            Vector3 unibk = joints[bk.first]-joints[bk.second];
            unibk = unibk/sqrt(unibk*unibk);
            
            Vector3 xprod = cross_product(unibi,unibk);
            if (xprod*xprod < LIMINF) {
               xprod = cross_product(unibi,Vector3(0,0,1));
            }
            if (xprod*xprod < LIMINF) {
               xprod = cross_product(unibi,Vector3(0,1,0));
            }
            
            c[i][j] = k+bones.size();
            atbones[k] = xprod;
         }
      }
   }
   
   getBonesOrderList();
}

void Skeleton::getBonesOrderList() {
   
   orderList.resize(bones.size());
   vector<int> posOrderList(bones.size());
   bool exchangeflag = false; int k = 0;
   for (unsigned int i=0; i<bones.size(); ++i) posOrderList[i] = i;
   
   do {
      exchangeflag = false;
      k++;
      for (unsigned int i=0; i<bones.size(); ++i) {
         //~ cout<<"i: "<<i<<endl;
         for (unsigned int j=0; j<controller[i].size(); ++j) {
            int ctemp;
            if (controller[i][j] > bones.size()) {
               ctemp = controller[i][j] - bones.size();
               if (posOrderList[ctemp]>posOrderList[i]) {
                  int tmp = posOrderList[i];
                  posOrderList[i] = posOrderList[ctemp];
                  posOrderList[ctemp] = tmp;
                  exchangeflag = true;
               }
            }
         }
      }
      
      //~ for (int i=0; i<orderList.size(); ++i) {
         //~ cout<<"pol: "<<i<<" : "<<posOrderList[i]<<endl;
      //~ }
   } while ((exchangeflag) and k <= bones.size());
   
   for (unsigned int i=0; i<orderList.size(); ++i) {
      int j = posOrderList[i];
      orderList[j] = i;
   }
   
   //~ for (unsigned int i=0; i<orderList.size(); ++i) {
      //~ cout<<"orderList[i]: "<<orderList[i]<<endl;
   //~ }
   //~ for (unsigned int i=0; i<orderList.size(); ++i) {
      //~ cout<<"posOrderList[i]: "<<posOrderList[i]<<endl;
   //~ }
}

void Skeleton::saveAs(const char *filename) {
   
   if (filename==0) return;
      
   std::ofstream fileout(filename);
   if (fileout) { 
      fileout<<"SKEL"<<endl;
      fileout<<bones.size()<<endl;
      for (unsigned int i=0; i<bones.size(); ++i) {
         const Bone &b = bones[i];
         const Point3 &p = joints[b.first];
         fileout<<b.first<<" "<<p[0]<<" "<<p[1]<<" "<<p[2]<<" "<<b.second<<endl;
      }
   }
   fileout.close();
   
   // the skel-format has two parts:
   // (1) head
   //     SKEL the skel format identifier
   //     j: number of joints
   // (2) joins geometry (j entries)
   //     id x y z idprev : the joint's, its geometric position and the previous joint id 
   // ... example
   // SKEL
   // 3 
   // 0 x0 y0 z0 -1
   // 1 x1 y1 z1 0
   // 2 x2 y2 z2 1
}

/// Reads an off model into a polyhedron data structure and returns it
/// @param filename Name of the file containing model
/// @return pointer to newly read model or 0 if unsuccessful
Skeleton* Skeleton::load(const char *filename) {
   
   std::fstream skel_file(filename, std::ios_base::in);
   if (skel_file) {
      Skeleton *s = new Skeleton(filename);
      return s;
   }
   
   return 0;
}

void Skeleton::buildGraph() {
   
   graph.clear();
   graph.resize(joints.size());
   // Insert the bones edges
   for (unsigned int i=0; i<bones.size(); ++i) {
      const Bone &b = bones[i];
      if (b.second != -1)
         graph[b.first].push_back(b.second);
   }
   
   // Infer the another edges caring no insert duplicate entries
   for (unsigned int f=0; f<graph.size(); ++f) {
      const vector<int> &row = graph[f];
      for (unsigned int c=0; c<row.size(); ++c) {
         vector<int> &neighbords = graph[row[c]];
         if (find(neighbords.begin(), neighbords.end(), (int)f) == neighbords.end())
            neighbords.push_back(f);
      }
   }
}

void Skeleton::floodfill_Dijkstra(int vindex) {
   
   /// initializing 
   for (unsigned int i=0; i<joints.size(); ++i)
      d[i] = NINFINITY;
   
   d[vindex] = 0;
   priority_queue<Joint_id_d, vector<Joint_id_d>, compareJointDistance>   priority_queue;
   priority_queue.push(Joint_id_d (vindex, d[vindex]));
   
   while (not priority_queue.empty()) {
      Joint_id_d f = priority_queue.top();
      priority_queue.pop();
      
      const vector<int> &target = graph[f.id];
      const Point3 &pf = joints[f.id];
      for (unsigned int c=0; c<target.size(); ++c) {
         const Point3 &pc = joints[target[c]];
         const Vector3 fc(pf[0]-pc[0], pf[1]-pc[1], pf[2]-pc[2]);
         Number dfc = 0;
         if (k[f.id]) dfc = 100000; 
         else         dfc = sqrt(fc*fc); 
         
         if (d[f.id] + dfc < d[target[c]]) {
            d[target[c]] = d[f.id] + dfc;
            priority_queue.push(Joint_id_d (target[c], d[target[c]]));
         }
      }
   }
}

void Skeleton::flipBone(unsigned int idbone) {
   
   if (idbone >= bones.size()) return;
   
   Bone &b = bones[idbone];
   Bone &bprev = bones[b.second];
   bprev.second = idbone;
   b.second = -1;
}

void Skeleton::newBone(unsigned int jointID, unsigned int jointIDprev) {
   
   if (jointID>=joints.size()) return;
   if (jointIDprev>=joints.size()) return;
   
   Bone &b = bones[jointID];
   b.second = jointIDprev;
}

void Skeleton::deleteBone(unsigned int idbone) {
   
   assert(idbone < bones.size());
   
   // delete the bone
   Bone &b = bones[idbone];
   int idfirst = b.first;
   int idsecond = b.second;
   b.second = -1;
   
   // delete the graph entry
   vector<int> &first = graph[idfirst];
   vector<int> &second = graph[idsecond];
   remove(first.begin(), first.end(), (int)idbone);
   remove(second.begin(), second.end(), (int)idbone);
}

void Skeleton::splitBone(unsigned int idbone) {
   
   assert(idbone < bones.size());
   
   // get the Bone
   Bone &b = bones[idbone];
   int idfirst = b.first;
   int idsecond = b.second;
   
   // compute the new joint position
   Point3 &pf = joints[idfirst];
   Point3 &ps = joints[idsecond];
   Number x = (pf[0] + ps[0])/2.0;
   Number y = (pf[1] + ps[1])/2.0;
   Number z = (pf[2] + ps[2])/2.0;
   
   // update geometry
   int i = newJoint(Point3(x,y,z));
   
   // insert the new bones
   newBone(idfirst, i);
   newBone(i, idsecond);
   
   // update the graph
   vector<int> n(2);
   n[0] = idfirst; n[1] = idsecond;
   graph.push_back(n);
   int id = graph.size()-1;
   
   vector<int> &first = graph[idfirst];
   vector<int> &second = graph[idsecond];
   replace(first.begin(), first.end(), idsecond, id);
   replace(second.begin(), second.end(), idfirst, id);
}

int Skeleton::newJoint(const Point3 &p) {
   
   joints.push_back(p);
   bones.push_back(Bone(bones.size(),-1));
   return joints.size()-1;
}

void Skeleton::deleteJoint(unsigned int idJoint) {
   
   assert(idJoint < joints.size());
   
   int idjoint = idJoint;
   // delete the joint entry
   vector<Point3>::iterator it = joints.begin();
   advance(it,idjoint);
   joints.erase(it);
   
   // delete the bone
   vector<Bone>::iterator itb = bones.begin();
   advance(itb,idjoint);
   bones.erase(itb);
   
   // delete the associated bones
   for (unsigned int i=0; i<bones.size(); ++i) {
      Bone &b = bones[i];
      if (b.first > idjoint)        b.first = b.first - 1; 
      if (b.second > idjoint)       b.second = b.second - 1; 
      else if (b.second == idjoint) b.second = -1; 
   }
   
   // build the graph
   buildGraph();
}

void Skeleton::rotate(const Point3 &o, const Vector3 &u, Number angle) {
   
   const Number cos_a = cos(angle);
   const Number sin_a = sin(angle);
   Vector3 uuv;
   for (unsigned int i=0; i<nJoints(); ++i) {
      Point3 &p = joints[i];
      Vector3 vp(p[0]-o[0],p[1]-o[1],p[2]-o[2]);
      uuv = u*(u*vp);
      vp = uuv + cos_a*(vp-uuv) + sin_a*(cross_product(u,vp));
      //~ p[0]=vp[0]+o[0]; p[1]=vp[1]+o[1]; p[2]=vp[2]+o[2];
      p = Point3(vp[0]+o[0], vp[1]+o[1], vp[2]+o[2]);
   }
}

void Skeleton::translate(const Vector3 &t) {
   
   for (unsigned int i=0; i<nJoints(); ++i) {
      Point3 &p = joints[i];
      p = p + t;
   }
}

void Skeleton::translateJoint(unsigned int i, const Vector3 &t) {
   
   Point3 &p = joints[i];
   p = p + t;
}

float Skeleton::getDistance(const float *v, unsigned int ib, float &t) {
   
   const Bone &bo = bones[ib];
   const Point3 &a = joints[bo.first];
   const Point3 &b = joints[bo.second];
   
   const Vector3 &ba = Vector3(b[0]-a[0],b[1]-a[1],b[2]-a[2]);
   const Vector3 &va = Vector3(v[0]-a[0],v[1]-a[1],v[2]-a[2]);
   const Vector3 &vb = Vector3(v[0]-b[0],v[1]-b[1],v[2]-b[2]);
   
   const Number &norma2aibi = ba*ba;
   const Number &dotvaibiai = va*ba;
   
   Number dminvaibi;
   if (dotvaibiai<=0)                  { dminvaibi = sqrt(va*va); t=1.0;}
   else if (dotvaibiai>=norma2aibi)    { dminvaibi = sqrt(vb*vb); t=0.0;}
   else                                { 
      dminvaibi = sqrt(va*va - ((dotvaibiai*dotvaibiai)/norma2aibi)); 
      t = 1.0-dotvaibiai/norma2aibi;
   }
   
   return dminvaibi;
}

float Skeleton::getEllipDistance(const float *ve, unsigned int ib, float &t) {
   
   const Point3 v(ve[0],ve[1],ve[2]);
   const Bone &bo = bones[ib];
   const Point3 &p = joints[bo.first];
   const Point3 &q = joints[bo.second];
   const Point3 o((p[0]+q[0])*0.5,(p[1]+q[1])*0.5,(p[0]+q[0])*0.5);
   
   const Vector3 vp(v,p);
   const Vector3 vq(v,q);
   const float &d1 = sqrt(vp*vp);
   const float &d2 = sqrt(vq*vq);
   float a = (d1 + d2)*0.5;
   
   const Vector3 pq(p,q);
   const Vector3 ov(o,v);
   
   float sproj = (pq*ov)/sqrt(pq*pq); // scalar projection of ov onto pq
   t = ((sproj/a)+1.0)*0.5;
   
   return 0;
}

Vector3 Skeleton::getBoneProjector(const float *v, unsigned int ib) {
   
   const Bone &bo = bones[ib];
   const Point3 &a = joints[bo.first];
   const Point3 &b = joints[bo.second];
   
   const Vector3 &ba = Vector3(b[0]-a[0],b[1]-a[1],b[2]-a[2]);
   const Vector3 &va = Vector3(v[0]-a[0],v[1]-a[1],v[2]-a[2]);
   const Vector3 &vb = Vector3(v[0]-b[0],v[1]-b[1],v[2]-b[2]);
   
   const Number &norma2aibi = ba*ba;
   const Number &dotvaibiai = va*ba;
   
   if (dotvaibiai<=0)                  { return va; }
   else if (dotvaibiai>=norma2aibi)    { return vb; }
   else {
      Number t = dotvaibiai/norma2aibi;
      return (Vector3(v[0],v[1],v[2]) - (Vector3(b[0],b[1],b[2])*t + Vector3(a[0],a[1],a[2])*(1.0-t)));
   }
}

Point3 Skeleton::getPointOnBone(unsigned ib, float t) {
   
   const Bone &bo = bones[ib];
   const Point3 &a = joints[bo.first];
   const Point3 &b = joints[bo.second];
   
   return Point3 ((a[0])*t + (b[0])*(1.0-t), 
                  (a[1])*t + (b[1])*(1.0-t), 
                  (a[2])*t + (b[2])*(1.0-t));
}

void Skeleton::computeBonesDistanceTable() {
   
   assert(nBones() > 0);
   
   cout<<"computeBonesDistanceTable(): "<<nBones()<<endl;
   dbones.resize(nBones());
   for (unsigned int i=0; i<dbones.size(); ++i) {
      dbones[i].resize(nBones(), 0.0);
   }
   
   for (unsigned int i=1; i<nBones(); ++i) {
      const Bone &bi = bones[i];   // initial bone
      Vector3 di = joints[bi.second] - joints[bi.first];
      Number ndi = 0.5*sqrt(di*di);
      floodfill_Dijkstra(bi.first);
      vector<Number> dist = d;
      for (unsigned int j=1; j<nBones(); ++j ) {
         if (i == j) continue;
         const Bone &bj = bones[j];   // initial bone
         if (d[bj.first] < d[bj.second])
            dbones[i][j] = d[bj.first];
         else 
            dbones[i][j] = d[bj.second];
      }
      floodfill_Dijkstra(bi.second);
      for (unsigned int j=1; j<nBones(); ++j) {
         if (i == j) continue;
         const Bone &bj = bones[j];   // initial bone
         Vector3 dj = joints[bj.second] - joints[bj.first];
         Number ndj = 0.5*sqrt(dj*dj);
         if (d[bj.first] < d[bj.second])
            dbones[i][j] = min(dbones[i][j], d[bj.first]);
         else 
            dbones[i][j] = min(dbones[i][j], d[bj.second]);
         //~ if (d[bj.first] < d[bj.second])
            //~ dbones[i][j] = min(dbones[i][j], d[bj.first]) + ndi + ndj;
         //~ else 
            //~ dbones[i][j] = min(dbones[i][j], d[bj.second]) + ndi + ndj;
      }
   }
   
   //~ for (unsigned int i=0; i<dbones.size(); ++i) {
      //~ for (unsigned int j=0; j<dbones.size(); ++j) {
         //~ cout<<"("<<i<<","<<j<<")"<<dbones[i][j]<<" ";
      //~ }
      //~ cout<<endl;
   //~ }
   cout<<"computeBonesDistanceTable(): end"<<endl;
}

float Skeleton::getDistanceBetweenBone(unsigned int i, unsigned int j) {
   
   return dbones[i][j];
}

Point3 Skeleton::getEllipPointOnBone(unsigned int ib, float t) {
   
   const Bone &bo = bones[ib];
   const Point3 &p = joints[bo.first];
   const Point3 &q = joints[bo.second];
   
   Vector3 pq(p,q);
   pq = pq*t;
   
   return p + pq;
}

//~ } // end MLS namespace
